/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/tape_half_auto.h
 * @Description  : 定义了TapeHalfAuto类，用于磁导的半自动导航控制。 
 *                 包含多个枚举类型、结构体以及用于处理输入信号、控制速度和状态转换的方法。
 *                 主要功能包括初始化、读取参数、获取传感器数据、状态管理及速度控制等。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:10:40
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "timestamp.h"
#include "follow_path.h"
#include <mutex>
#include <thread>
#include <vector>
#include <agv_msgs/msg/magnet_info.hpp>
#include <agv_srvs/srv/set_operation_mode.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include "public_macro.h"
// 急停
constexpr const char EMG_BUTTON[] = "emergencyButton";
// 停止按钮
constexpr const char STOP_BUTTON[] = "stopButton";
// 放行按钮
constexpr const char RELEASE_BUTTON[] = "releaseButton";
// 磁地标传感器
constexpr const char MAG_MARK_IO[] = "magDetect";

// The class  of class "TapeHalfAuto".
class TapeHalfAuto {
public:
    TapeHalfAuto();
    ~TapeHalfAuto(); // 析构函数

    // 初始化
    bool Init();

    // 半自动运行阶段
    enum HalfRunStage {
        Stage_Manual,
        Stage_HalfAgain,
        Stage_HalfNormalRun,
        Stage_HalfFinish,
        Stage_ForceFinish
    };

    // 磁信号状态
    enum MagSignal {
        SIGNAL_VALID,
        SIGNAL_INVALID
    };

    // 完成状态
    enum FinishStatus {
        Normal_Finish,
        Force_Finish,
        Error_Finish,
        Not_Finish
    };

    // 停止类型
    enum StopType {
        Type_InitStatus = 0, // 使用 RFID 或磁导航初始化状态
        Type_Mark
    };

    // 使用模式
    enum UseMode {
        BtnMode = 0, // 按钮触发上线模式
        ManualVelMode // 手柄发送速度模式
    };

    // 跟线停止标识和速度参数
    struct HalfRunParam {
        float auto_vel;
        int stop_type;
        int use_mode;

        HalfRunParam() {
            auto_vel = 0.2f;
            stop_type = static_cast<int>(StopType::Type_InitStatus);
            use_mode = static_cast<int>(UseMode::BtnMode);
        }
    };

    using MODE_REQUEST = agv_srvs::srv::SetOperationMode::Request;   // 定义操作模式请求
    using INPUT_IO_TYPE = DataTimeStamp<agv_msgs::msg::PairTypeInt>; // 定义输入IO点类型
private:
    void CycleRoutine(); // 检测二维码丢失逻辑
    void ThreadFunc();   // 线程函数
    void InitIo();       // 初始化IO对象
    void Reset();        // 复位所有变量
    void ReadParam();    // 读取参数
    void GetMagnetDataCB(const agv_msgs::msg::MagnetInfo& data); // 获取磁导航数据信息回调函数
    void GetJoyVelCB(const geometry_msgs::msg::Twist& data);     // 获取手柄速度数据回调函数
    void FindLevelParam(float v, float& a1, float& a2);   // 根据速度查找纠偏系数
    ModifyCtrl GenAngularVel(float vel);      // 计算纠偏量
    void LimitModifyV(const Velocity& plan_vel, ModifyCtrl& modify);    // 限制纠偏量
    void NormalOutVel(Velocity& control_vel, ModifyCtrl& modify_ctrl); // 设置叠加速度输出
    float GetRealVel(float timeout = 500);    // 获取里程计的实际速度
    float IntegralDis(float vel);             // 积分获取距离
    bool SwitchMode(const std::string mode); // 切换操作模式
    bool InputIo(INPUT_IO_TYPE& inputIo);     // 检测输入IO信号是否触发
    void ReadInputNewCallBack(const agv_msgs::msg::ReadInPutsNew& msg);   // 获取新IO输入信号回调函数
    void UpdateInputIoNew(const agv_msgs::msg::ReadInPutsNew& msg, INPUT_IO_TYPE& io); // 更新输入IO对象
    void SetVel(const Velocity& vel); // 设置半自动上线速度命令

    // 状态转换
    bool StageManualToHalfAuto();     // 是否可以切换模式；手动下->切换到半自动
    bool StageHalfReadyAgain();       // 是否进入巡线模式
    int StageNormalHalfRun(float target_v); // 是否初始化磁导航完成
    bool StageFinishToManual();       // 是否切换回由半自动->手动
    bool StageFinishToAuto();         // 完成跳到自动模式
    bool NeedStop();                  // 是否需要中断
    bool NeedForceFinish();           // 是否需要提前结束
    bool NormalFinish();              // 正常完成
    bool ErrorFinish();               // 错误完成
    bool RunModeMisMatch();           // 运行模式不匹配；无法运行在该逻辑内
    bool Steering(float target_v);    // 打舵
    void LogMark(INPUT_IO_TYPE& mark);// 地标日志打印
    void ChangeNavMode(USHORT mode);  // 切换导航模式
    
    bool initialized_;                // 初始化标识
    std::thread thread_;              // 线程句柄
    std::timed_mutex quit_mutex_;     // 线程退出互斥锁
    std::mutex data_mutex_;           // 数据操作互斥锁

    // 订阅
    rclcpp::Subscription<agv_msgs::msg::MagnetInfo>::SharedPtr sub_magnet_data_; // 磁导航偏差信息
    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_joy_vel_;     // 手柄速度
    rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr sub_io_new_;   // 新的IO输入信号

    // 切换操作模式
    rclcpp::Client<agv_srvs::srv::SetOperationMode>::SharedPtr op_mode_client_;

    // 当前数据
    DataTimeStamp<agv_msgs::msg::MagnetInfo> magnet_info_; // 磁导航偏差数据
    DataTimeStamp<geometry_msgs::msg::Twist> joy_vel_;     // 手柄速度数据

    // 半自动运行阶段
    HalfRunStage current_stage_;
    ModifyCtrl modify_ctrl_;          // 纠偏量

    float current_dead_size_;         // 当前里程计计算走过的长度(m)
    MagSignal signal_stage_;          // 地标信号的有效性

    // 输入信号触发
    INPUT_IO_TYPE emg_btn_;           // 急停信号
    INPUT_IO_TYPE stop_btn_;          // 停止信号
    INPUT_IO_TYPE release_btn_;       // 放行信号
    INPUT_IO_TYPE mag_detect_io_;     // 磁导航地标信号

    // 手动命令发送
    std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;  // 当前的控制速度
    Velocity vel_;                    // 上一次的工作模式
    AgvWorkMode last_mode_;           // 巡线参数
    HalfRunParam run_param_;          // 跟线参数
};